//Created 1/20/2015 by Christopher Allum
/*

CHANGE LOGS
before 2015   Christopher Allum
        Created
1/28/2015    Christopher Allum
        Began integration of the EMG.h class into main E-mu code and refactored input 
        parameters
2/17/2015    Christopher Allum
        Creating a new print statement specialized to be the output to Processing Gui.
        Changed the sensor data from type double to float throughout other classes and this one
2/28/2015    Christopher Allum
        Having a really hard time getting the IMU data. Not sure what's up. I fixed a few of the
        warnings that are only visible in verbose mode.  Changed pattern attributes to type double.
        I may have figured out the IMU issue.... Something to do with unplugging it and plugging it back in. :p
3/23/2015    Christopher Alllum
        Added in modes. created skeleton for next phases such as calibration mode and record mode.
        Removed extraneous code
3/28/2015    Christopher Allum
        Worked on some debugging. Added default states to calibration mode. starting a record mode gui.
        Made a comment to show the data array contents for each mode
*/
#include <SPI.h> // Included for SFE_LSM9DS0 library
#include <Wire.h>
#include <SFE_LSM9DS0.h>
#include "IMU.h"
#include <SoftwareSerial.h>  
#include "EMG.h"
#include <Pattern.h>
#include "patternRecog.h"
#define numSensorInputs 8
#define caliWindowSize 500
#define recordDelay 40
#define BUZZ_VOLUME 20
//==============DATA=====================================
#define numWatches 181 


//MODE 0 data
  //1-179: not used
  //180: mode

//MODE 1 data[]
  //   0    1        2      3    4    5      6        7        8        9        10          
  //[jerkX, jerkY, jerkZ, rotX, rotY, rotZ, emgAmp1, emgAmp2, emg1RMS, emg2RMS, emg1PeakFreq, 
  //  11           12            13             14              15            16              17
  //emg2PeakFreq, pattern1Score, pattern2Score, pattern3Score, pattern4Score, pattern5Score, pattern6Score
  //FFT data 1: 20-99
  //FFT data 2: 100-179
  //180: mode
  
//MODE 2 data[]
  //   0    1        2      3    4    5      6        7        8        9        10          
  //[jerkX, jerkY, jerkZ, rotX, rotY, rotZ, emgAmp1, emgAmp2, emg1RMS, emg2RMS, emg1PeakFreq, 
  //  11           12        13        14        15       16           17        18
  //emg2PeakFreq, jerkSens, rotSens, ampSens, rmsSens, modSens,     jerkTotAvg, rotTotAvg,
  //  19        20         21          22
  //ampTotAvg,  rmsTotAvg,  modTotAvg
  
  //23-179: not used
  //180: mode

//MODE 3 data[]
  //   0    1        2      3    4    5      6        7        8        9        10          
  //[jerkX, jerkY, jerkZ, rotX, rotY, rotZ, emgAmp1, emgAmp2, emg1RMS, emg2RMS, emg1PeakFreq, 
  //  11           12            13      
  //emg2PeakFreq,  gestureNum,  sampleNum
  
  
  //14-179: not used
  //180: mode



float data[numWatches];//a watch being defined as a variable watched through processing gui
//================================================================
//PATTERN RECOGNITION VARIABLES
#define numS 30 //number of samples per pattern -- only used for pattern creation
#define numF 12 //number of features per row of each pattern
float patternRecogInput[numF];//a watch being defined as a variable watched through processing gui
//================================================================
int bluetoothTx = 3;  // TX-O pin of bluetooth papasmurf
int bluetoothRx = 4;  // RX-I pin of bluetooth papasmurf

int buttonPin = 22;
double buttonPress;
int buttonTimer = 0;

int progress = 0;//used for recording mode
int caliTimer = 0;
int caliTimeMax = 700;
boolean doneCalibrating = false;
int caliThreshold = 50;


boolean showGUI = 1;//this print allows data to the Processing GUI
int mode = 1;//initialized at sleep mode
boolean isHigh = false;
boolean fallEdge = false;

  //sensitivity goals:  
    //define here the ideal ranges for each sensor
    //ideally, just rate each feature from 1 to 6, 6 being useful
    float jerkWeight= 6;  
    float rotWeight = 7;   
    float ampWeight = 1; 
    float rmsWeight = 5; 
    float modWeight = 1; 
  //==================================================================
  
//DEFAULT SENSITIVITIES
#define DEFAULT_AMP_SENS 0.0125    //   1 / 80
#define DEFAULT_RMS_SENS 0.00714   //   1 / 140
#define DEFAULT_MOD_SENS 0.003846   //   1 / 26
#define DEFAULT_ROT_SENS 0.0500   //   1 / 120
#define DEFAULT_JERK_SENS 25
/*
#define DEFAULT_AMP_SENS 1   
#define DEFAULT_RMS_SENS 1  
#define DEFAULT_MOD_SENS 1  
#define DEFAULT_ROT_SENS 1  
#define DEFAULT_JERK_SENS 1
*/
float ampSens = DEFAULT_AMP_SENS;
float rmsSens = DEFAULT_RMS_SENS; 
float modSens = DEFAULT_MOD_SENS;
float rotSens = DEFAULT_ROT_SENS;
float jerkSens = DEFAULT_JERK_SENS;
//CALIBRATION PARAMETERS
  float jerkAvg;//average
  float rotAvg;
  float ampAvg;
  float rmsAvg;
  float modAvg;
  
  float jerkVar = 0;//variance
  float rotVar = 0;
  float ampVar = 0;
  float rmsVar = 0;
  float modVar = 0;  
  
  
  float prevJerk = 0;
  float prevRot = 0;
  float prevAmp = 0;
  float prevRms = 0;
  float prevMod = 0;
  
  float deltaJerk[caliWindowSize];
  float deltaRot[caliWindowSize];
  float deltaAmp[caliWindowSize];
  float deltaRms[caliWindowSize];
  float deltaMod[caliWindowSize];
  
  
  float jerkBuff[caliWindowSize];
  float rotBuff[caliWindowSize];
  float ampBuff[caliWindowSize];
  float rmsBuff[caliWindowSize];
  float modBuff[caliWindowSize];
  int calIter = 0;
  
  boolean resetCalibration = true;//
  

  void boop(int numBoops){
    int buzzValue = BUZZ_VOLUME;
    for(int i = 0; i < numBoops; i++){
       analogWrite(20,buzzValue); 
       delay(100);
       analogWrite(20,0);
       delay(100);
    }  
  }
  
void printButtonVal(){
  Serial.println("=================================");
  Serial.print("  button timer: ");
  Serial.println(buttonTimer);

  Serial.print("  isHigh: ");
  Serial.println(isHigh);
  
  Serial.print("  fallEdge: ");
  Serial.println(fallEdge);
  
  Serial.print("  mode: ");
  Serial.println(mode);
  Serial.println("=================================");
}

void getMode(){
  //modes:
  //  0     1       2         3
  //sleep  run  calibrate  record
  //READ SWITCHES
  buttonPress = analogRead(buttonPin);
  
  //===ENTER CALIBRATION==
  if((buttonTimer > 40)){
        boop(3);
        mode = 3;//enter recording
        buttonTimer = 0;
        fallEdge = false;
        isHigh = false;
        return;
   }
   //=====================
   
  // printButtonVal(); 
  
  //If you're on the falling edge of the button then determine what to do 
  if((fallEdge == true)){ //then cycle the state
      //Serial.println("falling edge ACTIVATED");
     if(mode == 0){
       mode = 1;
       boop(2);
     }
     else if(mode == 1){
       mode = 0;
       boop(1);
     }
     //else if(mode == 2){
     // mode = 1;
     // boop(2); 
     //}
     
     buttonTimer = 0;
     fallEdge = false;
     isHigh = false;  
     return;
  }
  
  if(buttonPress == 1023){
    isHigh = true;
    fallEdge = false;
  }
  
  if(isHigh)
    buttonTimer++;
    
  if(isHigh && (buttonPress != 1023)){//if it is high but button is low then set low and reset variables, but set fallEdge to true
    buttonTimer = 0;
    isHigh = false;
    fallEdge = true;
    getMode();
  }
  
}

void setup(){ 
  //delay(8000);
  //Serial.println("BEGINNING");
  Serial.begin(115200);   
  Serial1.begin(115200);//for bt
  Wire.begin();
  dof.begin();
  initializeEMG(); 
  analogWrite(23,255);
  initializePatterns();
}

void loop() {
  
  //Serial.println("running");
  //delay(50);
  //getMockData(data,numWatches)
  getMode();
  //SLEEP MODE
  if(mode == 0){ //sleep mode
  //Serial.println("in mode 0");
    data[180] = mode;//set the mode

    if(!showGUI){
      //Serial.println("zzz   =.=   zzz");
      //delay(1000);
    }
  }
  //RUN MODE
  if(mode == 1){ 
//  Serial.println("in mode 1");    
      //Serial.println("run mode"); 
      data[180] = mode;//set the mode
      readIMU(data, rotSens, jerkSens);
      //Serial.println("read the IMU");
      readEMG(data, ampSens, rmsSens, modSens);
      //Serial.println("read both sensors");
      loadPatternRecogInput();
      compareData(patternRecogInput);
      writePAT_ToPrc(data);
  }
  //CALIBRATION MODE
  if(mode == 2){//calibrate mode
    //Serial.println("in mode 2");
    data[180] = mode;//set the mode
    //mode = 3;
    //Serial.println("calibrating");
    //do this once to get it into the calibration screen      
    
    calibrateSens();
  }
  if(mode!=2){//allow the calibration temp variables to reset when you leave calibration mode
    resetCalibration = true;
  }
  
  //RECORD MODE
  if(mode == 3){//record mode
   boop(4);
    data[180] = mode;//set the mode 
    if(showGUI){
      recordGui();
    }
    mode = 1;
    //commandline record for manually getting the data

    
  }//end mode
  if(showGUI && (mode != 3)){
      printArray(data, numWatches); 
  } 

}


void loadPatternRecogInput(){
  for(int i = 0; i < numF; i++){
    patternRecogInput[i] = data[i];
  }
}

void getMockData(float* output, int numData){
  for(int i = 0; i < numData; i++){
    output[i] = random(-3,3);
  }
}

void calibrateSens(){
  
  
  //if just entered calibration mode, reinitialize the variables 
    if(resetCalibration){
      doneCalibrating = false;
      calIter = 0;
      jerkAvg = 0;
      rotAvg = 0;
      ampAvg = 0;
      rmsAvg = 0;
      modAvg = 0;
      ampSens = DEFAULT_AMP_SENS;
      rmsSens = DEFAULT_RMS_SENS;
      modSens = DEFAULT_MOD_SENS;
      rotSens = DEFAULT_ROT_SENS;
      jerkSens = DEFAULT_JERK_SENS;
      resetCalibration = false;
      caliTimer = 0;
      for(int i = 0; i<caliWindowSize;i++){//reset window
        jerkBuff[i] = jerkWeight / DEFAULT_JERK_SENS;
        rotBuff[i] = rotWeight / DEFAULT_ROT_SENS;
        ampBuff[i] = ampWeight / DEFAULT_AMP_SENS;
        rmsBuff[i] = rmsWeight / DEFAULT_RMS_SENS;
        modBuff[i] = modWeight / DEFAULT_MOD_SENS;
      }
    }
    
  //perform a reading
  readIMU(data, DEFAULT_ROT_SENS, DEFAULT_JERK_SENS);//read with a constant sensitivity of default to avoid making a feedback loop   
  readEMG(data, DEFAULT_AMP_SENS, DEFAULT_RMS_SENS, DEFAULT_MOD_SENS);
  //find the average of each sensor type
  jerkAvg = sqrt(data[0]*data[0] + data[1]*data[1] + data[2]*data[2]);
  rotAvg = sqrt(data[3]*data[3] + data[4]*data[4]+ data[5]*data[5]);
  ampAvg = sqrt(data[6]*data[6] + data[7]*data[7]);
  rmsAvg = sqrt(data[8]*data[8] + data[9]*data[9]);
  modAvg = sqrt(data[10]*data[10] + data[11]*data[11]);
  
  //load the averages into a moving window
  jerkBuff[calIter] = jerkAvg;
  rotBuff[calIter] = rotAvg;
  ampBuff[calIter] = ampAvg;
  rmsBuff[calIter] = rmsAvg;
  modBuff[calIter] = modAvg;
  
  if(calIter < caliWindowSize-1){
    calIter++;
  }
  else{
    calIter = 0;
  }
  
  //the avg value of the window becomes the reading to set the sensitivity
  float jerkTotalAvg = 0;
  float rotTotalAvg = 0;
  float ampTotalAvg = 0;
  float rmsTotalAvg = 0;
  float modTotalAvg = 0;
  for(int i = 0; i < caliWindowSize; i++){
    jerkTotalAvg += jerkBuff[i]; 
    rotTotalAvg += rotBuff[i]; 
    ampTotalAvg += ampBuff[i]; 
    rmsTotalAvg += rmsBuff[i]; 
    modTotalAvg += modBuff[i]; 
  }
  
  jerkTotalAvg/= caliWindowSize; 
  rotTotalAvg /= caliWindowSize;  
  ampTotalAvg /= caliWindowSize; 
  rmsTotalAvg /= caliWindowSize; 
  modTotalAvg /= caliWindowSize; 
   
  ampSens  = ampWeight / ampTotalAvg;
  rmsSens  = rmsWeight / rmsTotalAvg;
  modSens  = modWeight / modTotalAvg;
  rotSens  = rotWeight / rotTotalAvg;
  jerkSens = jerkWeight/jerkTotalAvg;


//the two lines above update data[0 - 11]

   //write into sensitivities
     data[12] = jerkSens;
     data[13] = rotSens;
     data[14] = ampSens;
     data[15] = rmsSens;
     data[16] = modSens;
     data[17] = jerkTotalAvg;
     data[18] = rotTotalAvg;
     data[19] = ampTotalAvg;
     data[20] = rmsTotalAvg;
     data[21] = modTotalAvg;
     
     //determine variance to decide whether or not you're done calibrating
     
   caliTimer++;

//decide if done by iterating through the delta array. 
//If all changes in sens are less than some threshold then you're done

  //read so that the gui gets the info soon after.
  readIMU(data, rotSens, jerkSens);
  readEMG(data, ampSens, rmsSens, modSens);
  
  if(caliTimer >= caliTimeMax)
    doneCalibrating = true;
    
  if(doneCalibrating){
    resetCalibration = true;
    mode = 3;
  }
}

void recordGui(){
  float tempData[numS*numF*6];  
  int gestureNum = 0;
  for(gestureNum = 0; gestureNum < 6; gestureNum++){
    boop(4);
    data[12] = gestureNum;

    for(int row = 0; row < numS; row++){
      delay(recordDelay);//delay between samples
      readIMU(data, rotSens, jerkSens);
      readEMG(data, ampSens, rmsSens, modSens);
      for(int col = 0; col < numF; col++){
        tempData[gestureNum*numF*numS  +  row*numF  +  col] = data[col];
        
        //Serial.print("  writing into position at: ");
        //Serial.println(gestureNum*numF*numS  +  row*numF  +  col);
      }
      data[13] = row;
      printArray(data, numWatches); 
    }
    boop(4);
    delay(recordDelay*5);
  }

  writePatternAttrib(tempData);
}

